/* Printer */
int CLK_PIN = 2;
int IN_PIN = 3;
int OUT_PIN = 4;
int LED_PIN = 13;
 int DELAY = 60;

/* Stepper */ 
int STEP1_PIN = 8; 
int STEP2_PIN = 9; 
int STEP3_PIN = 10; 
int STEP4_PIN = 11; 

int STEP_EN1_PIN = 6;
int STEP_EN3_PIN = 7;

int STEP_DELAY = 15; 

int PULL_STEPS = 7;

int SNAPBACK_DELAY = 600;

byte line[640] = {0,0,0,0,0,0,0,0,0,0,3,3,3,3,3,3,0,0,0,0,0,0,0,0,0,0,99,99,140,140,8,8,0,0,4,4,4,4,4,4,12,12,143,143,140,140,136,136,0,0,0,0,0,0,0,0,0,0,131,131,140,140,216,216,0,0,0,0,64,64,64,64,192,192,193,193,204,204,137,137,0,0,0,0,12,12,8,8,8,8,152,152,152,152,152,152,0,0,0,0,0,0,0,0,0,0,136,136,136,136,136,136,0,0,0,0,0,0,0,0,0,0,102,102,108,108,56,56,0,0,0,0,0,0,0,0,0,0,12,12,100,100,76,76,0,0,0,0,0,0,0,0,4,4,8,8,12,12,12,12,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,24,24,200,200,136,136,0,0,0,0,98,98,102,102,70,70,68,68,196,196,196,196,0,0,0,0,0,0,0,0,0,0,40,40,76,76,204,204,0,0,0,0,0,0,0,0,0,0,32,32,33,33,49,49,0,0,128,128,192,192,192,192,128,128,240,240,153,153,137,137,0,0,0,0,196,196,192,192,128,128,140,140,140,140,136,136,0,0,0,0,0,0,0,0,0,0,204,204,100,100,204,204,0,0,32,32,32,32,32,32,32,32,110,110,120,120,112,112,0,0,0,0,0,0,16,16,16,16,120,120,32,32,32,32,3,3,2,2,6,6,6,6,0,0,0,0,0,0,0,0,24,24,25,25,25,25,31,31,1,1,1,1,2,2,12,12,152,152,153,153,153,153,154,154,0,0,0,0,0,0,0,0,152,152,145,145,145,145,31,31,0,0,0,0,0,0,0,0,153,153,158,158,152,152,136,136,0,0,0,0,0,0,0,0,17,17,17,17,49,49,57,57,0,0,0,0,0,0,0,0,152,152,152,152,56,56,217,217,0,0,0,0,0,0,0,0,48,48,48,48,80,80,136,136,0,0,0,0,0,0,0,0,200,200,240,240,192,192,64,64,0,0,0,0,0,0,0,0,22,22,51,51,51,51,19,19,0,0,0,0,0,0,0,0,1,1,1,1,129,129,193,193,0,0,0,0,0,0,0,0,136,136,24,24,24,24,249,249,0,0,0,0,0,0,0,0,204,204,140,140,136,136,140,140,0,0,0,0,0,0,0,0,216,216,240,240,193,193,67,67,0,0,0,0,0,0,0,0,81,81,145,145,155,155,147,147,0,0,0,0,0,0,0,0,25,25,25,25,17,17,35,35,0,0,0,0,0,0,0,0,136,136,24,24,24,24,24,24,0,0,0,0,0,0,0,0,204,204,204,204,136,136,136,136,0,0,0,0,0,0,0,0,112,112,216,216,200,200,140,140,0,0,0,0,0,0,0,0,96,96,96,96,64,64,80,80,0,0,0,0,0,0,0,0};

byte rxByte; // Buffer for the last received byte

byte mask = 1;

int byteCnt = 0;

 
void doPull(){
	mActive();
	for(int i=0; i<PULL_STEPS; i++){
		mStep();
	}
	mLoose();
	
}
 
void mStep(){
   
    digitalWrite(LED_PIN, HIGH); 
     
    digitalWrite(STEP1_PIN, LOW); 
    digitalWrite(STEP2_PIN, HIGH); 
    digitalWrite(STEP3_PIN, HIGH); 
    digitalWrite(STEP4_PIN, LOW); 
    delay(STEP_DELAY); 
 
    digitalWrite(STEP1_PIN, LOW); 
    digitalWrite(STEP2_PIN, HIGH); 
    digitalWrite(STEP3_PIN, LOW); 
    digitalWrite(STEP4_PIN, HIGH); 
    delay(STEP_DELAY); 
 
    digitalWrite(STEP1_PIN, HIGH); 
    digitalWrite(STEP2_PIN, LOW); 
    digitalWrite(STEP3_PIN, LOW); 
    digitalWrite(STEP4_PIN, HIGH); 
    delay(STEP_DELAY); 
   
    digitalWrite(STEP1_PIN, HIGH); 
    digitalWrite(STEP2_PIN, LOW); 
    digitalWrite(STEP3_PIN, HIGH); 
    digitalWrite(STEP4_PIN, LOW); 
    delay(STEP_DELAY); 
    
        digitalWrite(LED_PIN, LOW); 
}

void mActive(){
      digitalWrite(STEP_EN1_PIN, HIGH);
        digitalWrite(STEP_EN3_PIN, HIGH);
}

void mLoose(){
    digitalWrite(STEP_EN1_PIN, LOW);
        digitalWrite(STEP_EN3_PIN, LOW);
  
     digitalWrite(STEP1_PIN, LOW); 
    digitalWrite(STEP2_PIN, LOW); 
    digitalWrite(STEP3_PIN, LOW); 
    digitalWrite(STEP4_PIN, LOW); 
}
 













void setup(){
  Serial.begin(19200); 

  Serial.println("SuperMario"); 


  /* Setup pins */
  pinMode(CLK_PIN, OUTPUT);                                               
  pinMode(OUT_PIN, OUTPUT);                                              
  pinMode(IN_PIN, INPUT); 

  digitalWrite(IN_PIN, HIGH);   // Internal pull-up for the Input port. IMPORTANT! 

  digitalWrite(CLK_PIN, HIGH);  // Reset clock to idle



	
  pinMode(STEP1_PIN, OUTPUT); 
  pinMode(STEP2_PIN, OUTPUT); 
  pinMode(STEP3_PIN, OUTPUT); 
  pinMode(STEP4_PIN, OUTPUT); 

  pinMode(STEP_EN1_PIN, OUTPUT); 
  pinMode(STEP_EN3_PIN, OUTPUT); 

  pinMode(LED_PIN, OUTPUT);


	mLoose();
}


void cmdFin(){
  unsigned int crc = 0;

  /* Line package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x04); // Command Type Enquiry
  crc+= 0x04;

  writeByte(0x00); // Compression. Off

  writeByte(0x00); 
  writeByte(0x00);




  writeInt(crc);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK




}


void cmdLine(){
  unsigned int crc = 0;
  
 /* Line package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x04); // Command Type Enquiry
  crc+= 0x04;
  
  writeByte(0x00); // Compression. Off

  writeByte(0x80); crc+=0x80;
  writeByte(0x02); crc+=0x02;

  for(int i = 0; i< 640; i++){

      byte val = line[i];
      
    
      writeByte(val);
      

      crc += val;
  
  }


  writeInt(crc);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK

  

 
}



void cmdEnq(){

  /* Sample package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x0F); // Command Type Enquiry
  writeByte(0x00); // Compression. Off

  writeByte(0x00);
  writeByte(0x00);

  writeInt(0x0F);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK
}

void cmdInit(){
  /* Init package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x01); // Command Type Enquiry
  writeByte(0x00); // Compression. Off

  writeByte(0x00);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x00);

  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK
}

void cmdPrnt(){
  /* Start printing package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x02); // Command Type Start Printing
  writeByte(0x00); // Compression. Off

  writeByte(0x04);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x13);

  writeByte(0xe4);
  writeByte(0x40);


  writeByte(0x3e);
  writeByte(0x01);


  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK 
}


void cmdPrntWo(){
  /* Start printing package */
  writeByte(0x88); // Sync #1
  writeByte(0x33); // Sync #2

  writeByte(0x02); // Command Type Start Printing
  writeByte(0x00); // Compression. Off

  writeByte(0x04);
  writeByte(0x00);

  writeByte(0x01);
  writeByte(0x00); //  margin

  writeByte(0xe4);
  writeByte(0x40);


  writeInt(299);


  writeByte(0x00);  
  writeByte(0x00); // Provoke ACK 
}

void doPrint(){
  cmdEnq();  // Unneccessary enquiry
  cmdInit(); // Init print process
  cmdLine(); // Send the line
  cmdFin();  // Send empty data packet
  cmdPrntWo(); // Do it! Damn it!
 


}

void loop(){
  if (Serial.available() > 0) {
    rxByte = Serial.read();
    line[byteCnt] = rxByte;  // Save received byte to buffer
    byteCnt++;
  }

  /* Print every 640 bytes. TODO: Establish some back-channel or full-blown protocol. Once this game gets messed up somehow, strange things will happen. */
  if(byteCnt == 640){
    doPrint();
	delay(SNAPBACK_DELAY);
	doPull();
    byteCnt = 0;  
  }
}


/**
 * Function to split one 16-bit (UInt16) int into two bytes and send them in the right order
 *
 */
void writeInt(unsigned int i){
  writeByte((byte)(i));   // Least Significant
  writeByte((byte)(i>>8));   // Most Significant
}


void writeByte(byte b)
{
  for(int i=0;i<8;i++) {  // Counting backwards 
    digitalWrite(CLK_PIN,LOW);   // Clock goes zero. Something might happen
    if(b & 0x80) {                                  
      digitalWrite(OUT_PIN,HIGH);                     //1
    } 
    else {
      digitalWrite(OUT_PIN,LOW);                      //0
    }
    b <<= 1;            // Bitshifting left out out OUT OUT
    delayMicroseconds(DELAY);
    digitalWrite(CLK_PIN,HIGH);                           // Clock goes HIGH again. The bit is over
    delayMicroseconds(DELAY);
  }
  digitalWrite(OUT_PIN,LOW);                          // GO LOW GO LOW

}





